/**
 *  core.c - Task management.
 *
 *  Copyright (C) 2008-2010 ZhangHu
 *  All rights reserved.
 *  E-MAIL: anmnmnly@gmail.com
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "include/core.h"

extern void init_device(void);// Initialize all devices
extern void Main(void);       // The entry routine for a application program.

static tcb_t idle_tcb;        // TCB of the idle task
static stk_t idle_stk[IDLE_TASK_STACK_SIZE]; // Stack of the idle task
static tcb_t init_tcb;        // TCB of the initial task
static stk_t init_stk[INIT_TASK_STACK_SIZE]; // Stack of the initial task

static bool_t is_lock_schedule;
static uint32 idle_cnt;        // Records the number of times of idle task
static list_t ptask[MAX_PRIO];  // Ready queue
static list_t psleep;           // Sleep queue
static list_t pdelay;           // Delay queue
static list_t *pglobal;         // A temporary pointer used by scheduler()
static uint32 highprio;        // The hightest priority
static list_t task;             // The head of all tasks chain
static uint32 sysclock;        // System clock count


tcb_t *pnewtcb; // Pointer to the TCB of the next running task
tcb_t *pcurtcb; // Pointer to the TCB of running task


/* The following variables and arrays used to find out
 * the highest priority in all tasks.
 */
static uint32 prio_rdy_grup;
static uint32 prio_rdy[READY_NUM];
const uint32 prio_mask[] =  {
    0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80
};
const uint32 prio_decision[] =  {
    0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x0F
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x1F
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x2F
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x3F
    6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x4F
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x5F
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x6F
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x7F
    7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x8F
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0x9F
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0xAF
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0xBF
    6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0xCF
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0xDF
    5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,  // 0x00-0xEF
    4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0   // 0x00-0xFF
};



/**
 * chk_task_id - Check the existence of a task ID.
 * @task_id: A task ID.
 * @return: return FALSE if task id is exist.
 *
 * @notes: just traversal the task list, and Comparing the two task IDs.
 */
static bool_t chk_task_id(int32 task_id)
{
    tcb_t *ptcb;
    list_t *plist = task.next;

    if(plist == &task) {
        return(TRUE);
    }

    do {
        ptcb = mac_find_entry(plist, tcb_t, task_link);
        if(ptcb->id == task_id) {
            return(FALSE);
        }
        plist = plist->next;
    } while(plist != task.next);
    return(TRUE);
}



/**
 * get_max_prio - Get the max priority in ready queue.
 * @return: The hightest priority in ready queue.
 *
 * @notes:
 */
static uint32 get_max_prio(void)
{
    uint32 high;
    uint32 low;
    uint32 prio;

    high = prio_decision[prio_rdy_grup];
    low = prio_decision[prio_rdy[high]];
    prio = (high << 3) + low;
    return(prio);
}



/**
 * add_tcb_ready - Insert a TCB into ready queue according to it's priority.
 * @ptcb: pointer to the TCB that will be inserted to ready queue.
 * @return:
 *
 * @notes:
 */
void add_tcb_rdy(tcb_t *ptcb)
{
    uint32 prio;

    if(ptcb == NULL) {
        return;
    }

    prio = ptcb->prio;

    if((prio_rdy[prio >> 3] & prio_mask[prio & 0x07]) == 0) {
        prio_rdy_grup |= prio_mask[prio >> 3];
        prio_rdy[prio >> 3] |= prio_mask[prio & 0x07];
    }
    add_node_list_rear(&ptask[prio], &ptcb->link);
}



/**
 * del_tcb_rdy: Delete a TCB in ready queue according to it's start address.
 * @ptcb: Pointer to the TCB that will be deleted from ready queue.
 * @return: The link list address of deleted TCB.
 *
 * @notes:
 */
list_t *del_tcb_rdy(tcb_t *ptcb)
{
    uint32 prio;
    list_t *renode = NULL;

    if(ptcb == NULL) {
        return(NULL);
    }

    if((ptcb->id == IDLE_TASK_ID) && (ptcb->prio == IDLE_TASK_PRIO)) {
        return(NULL);
    }

    prio = ptcb->prio;
    renode = &ptcb->link;

    // TCBs have existed in ready queue
    if((ptask[prio].next != &ptask[prio]) &&
       (ptask[prio].prev != &ptask[prio])) {

        // There is only one TCB in ready queue currently
        if((ptask[prio].next == renode) && (ptask[prio].prev == renode)) {
            if((prio_rdy[prio >> 3] &= ~prio_mask[prio &0x07]) == 0) {
                prio_rdy_grup &= ~prio_mask[prio >> 3];
            }
        }

        return(del_node_list(&ptask[prio], renode));
    }

    return(NULL);
}



/**
 * add_tcb_sleep: Inserte a TCB to sleep queue.
 * @ptcb: Pointer to the TCB that will be inserted to sleep queue.
 * @return:
 *
 * @notes:
 */
void add_tcb_sleep(tcb_t *ptcb)
{
    if(ptcb == NULL) {
        return;
    }

    add_node_list_rear(&psleep, &ptcb->link);
}



/**
 * del_tcb_sleep: Delete a TCB from sleep queue.
 * @ptcb: Pointer to the TCB that will be deleted from sleep queue.
 * @return:
 *
 * @notes:
 */
list_t *del_tcb_sleep(tcb_t *ptcb)
{
    if(ptcb == NULL) {
        return(NULL);
    }

    return(del_node_list(&psleep, &ptcb->link));
}



/**
 * scheduler - Estimate whether higher task has ready, and decide
 *             which task will be excuted.
 * @return:
 *
 * @notes:
 */
void scheduler(void)
{
    mac_disable_irq();
    if(is_lock_schedule == TRUE) {
        mac_enable_irq();
        return;
     }

    if(int_flag() == TRUE) {
        int_nesting_sub();
    }

    if(int_nesting() != 0) {
        mac_enable_irq();
        return;
    }

    highprio = get_max_prio(); // get the hightest priority
    if((pcurtcb != NULL) && (pcurtcb->prio <= highprio) &&  \
        (pcurtcb->status == T_READY)) {
        pglobal = &pcurtcb->link;
    } else {
        pglobal = ptask[highprio].next;
    }

    do {
        if((pnewtcb = mac_find_entry(pglobal, tcb_t, link)) != NULL) {
            if(pnewtcb->exe_time >= pnewtcb->slice_time) {
                pnewtcb->exe_time = 0;
                pglobal = pglobal->next;
            } else {
                break;
            }
        }
    } while(1);

    if(pnewtcb != pcurtcb) {
        ptask[pnewtcb->prio].next = &pnewtcb->link;
        if(ptask[pnewtcb->prio].prev == &pnewtcb->link) {
            ptask[pnewtcb->prio].prev = (ptask[pnewtcb->prio].prev)->prev;
        }
        if(int_flag() == FALSE) {
            if(pcurtcb == NULL) {
                TaskStart();
            } else {
                TaskSwitch();
            }
        } else if(int_flag() == TRUE) {
            int_flag_clr();
            if(pcurtcb == NULL) {
                TaskIntStart();
            } else {
                TaskIntSw();
            }
        }
    } else if(int_flag() == TRUE) {
        int_flag_clr();
        TaskContinue();
    }
}



/**
 * idle_task - This task runs when there are no task ready.
 * @return:
 *
 * @note: This task is at ready status forever.
 */
static void idle_task(void)
{
    while(1) {
        mac_disable_irq();
        idle_cnt++;
        mac_enable_irq();
    }
}



/**
 * idle_task - Initial task.
 * @return:
 *
 * @note: This task is the first application task in Bycore.
 */
static void Init(void)
{
    init_device();
    Main();
    osSleep();
}



/**
 * cre_sys_task - Crate a TCB for new task.
 * @ptask: Pointer to start addrees of task.
 * @taskid: Task ID.
 * @prio: The priority of task.
 * @time: Slice time.
 * @pstk: Stack size that the task ask for.
 * @return:
 *
 * @notes:
 */
static int32 create_task(void(*pTask)(), tcb_t *pTcb, uint32 TaskID,
                          uint32 Prio, uint32 Time, int32 *pSpace,
                          uint32 StkSize)
{
    int32 *space = pSpace;
    tcb_t *tcb = pTcb;
    tcb->pstart = pSpace;

    if(chk_task_id(TaskID) == FALSE) {
        return(FALSE);
    }

    // Initialize the TCB space
    if((tcb == NULL) || (space == NULL) || (pTask == NULL) ||
       (Prio >= MAX_PRIO) || (Time == 0))
    {
        return(FALSE);
    }

#if STACK == DOWN
    space += StkSize - 1;
#endif

    *space = (int32)pTask; // Store the entry address of task
    tcb->context = space;

#if STACK == DOWN
    space--;
#else
    space++;
#endif

    // Initialize task stack
    init_context(space);

    tcb->id = TaskID;
    tcb->prio = Prio;
    tcb->slice_time = Time;
    tcb->exe_time = 0;
    tcb->delay_time = 0;
    tcb->status = T_READY;
    tcb->sem_link.next = NULL;

    // Add new TCB to ready queue
    add_tcb_rdy(tcb);
    add_node_list_rear(&task, &tcb->task_link);
    return(TRUE);
}



/**
 * sysinit - Initialize global varialbles and data structs
 * @return:
 *
 * @notes:
 */
void sysinit(void)
{
    uint32 i = 0;

    // Initialize all global variables
    is_lock_schedule = ((bool_t)FALSE);
    idle_cnt = 0;
    pcurtcb = NULL;
    pnewtcb = NULL;
    pglobal = NULL;
    highprio = 0;
    sysclock = 0;
    psleep.next = &psleep;
    psleep.prev = &psleep;
    pdelay.next = &pdelay;
    pdelay.prev = &pdelay;

    // initialize ptask[MAX_PRIO], make it point to itself.
    // ptask[i] points to the head and rear of ready queue,
    // all the priority of TCBs is 'i' in ready queue.
    for(i = 0; i < MAX_PRIO; i++) {
        ptask[i].next = &ptask[i];
        ptask[i].prev = &ptask[i];
    }


    // Initialize prio_rdy_grup and prio_rdy[].
    // we can easily get the TCB of the max priority task in
    // ready queue. This method accelerates seeking speed for
    // the max priority task.
    prio_rdy_grup = 0;
    for(i = 0; i < READY_NUM; i++) {
        prio_rdy[i] = 0;
    }

    initmem();  // Initialize memory management section
    initirq();  // Initialize interruption

    task.next = &task;
    task.prev = &task;

    // Create Init and idle task
    create_task(idle_task, &idle_tcb, IDLE_TASK_ID, IDLE_TASK_PRIO, 5,
                idle_stk, IDLE_TASK_STACK_SIZE);
    create_task(Init, &init_tcb, INIT_TASK_ID, INIT_TASK_PRIO, 20, init_stk,
                INIT_TASK_STACK_SIZE);
    init_tick();    // Initialize time tick
    scheduler();    // OK, Bycore is running!
}



/**
 * osCreateTask - Creation a new task.
 * @pTask: Pointer to start addrees of task.
 * @pTcb: Pointer to TCB of task
 * @TaskID: Task ID.
 * @Prio: The priority of task.
 * @Time: Slice time.
 * @pStk: Pointer to stack space.
 * @StkSize: Stack size.
 * @return:
 *
 * @notes:
 */
void osCreateTask(void(*pTask)(), tcb_t *pTcb, int32 TaskID, uint32 Prio,
                  uint32 Time, stk_t *pStk, uint32 StkSize)
{
    if(Prio >= MAX_PRIO) {
        return;
    }

    mac_disable_irq();
    create_task(pTask, pTcb, TaskID, Prio, Time, (int32*)pStk, StkSize);
    mac_enable_irq();
    scheduler();
}



/**
 * osInitTask - Creation a new task.
 * @pTask: Pointer to start addrees of task.
 * @TaskID: Task ID.
 * @Prio: The priority of task.
 * @Time: Slice time.
 * @StkSize: Stack size.
 * @return:
 *
 * @notes:
 */
void osInitTask(void(*pTask)(), int32 TaskID, uint32 Prio, uint32 Time,
                uint32 StkSize)
{
    tcb_t *ptcb;
    stk_t *stk;

    if(Prio >= MAX_PRIO) {
        return;
    }

    mac_disable_irq();
    ptcb = (tcb_t*)allocate(sizeof(tcb_t));
    if(ptcb == NULL) {
        mac_enable_irq();
        return;
    }

    stk = (stk_t*)allocate(StkSize *sizeof(stk_t));
    if(stk == NULL) {
        reclaim(ptcb);
        mac_enable_irq();
        return;
    }

    if((create_task(pTask, ptcb, TaskID, Prio, Time, stk, StkSize) == FALSE)) {
        reclaim(ptcb);
        reclaim(stk);
        mac_enable_irq();
        return;
    }
    mac_enable_irq();
    scheduler();
}



/**
 * ChgPrio - Change the priority of a task.
 * @NewPrio - The new priority.
 * @return:
 *
 * @notes:
 */
void osChgPrio(uint32 NewPrio)
{
    if((NewPrio == pcurtcb->prio) || (NewPrio >= MAX_PRIO)) {
        return;
    }

    mac_disable_irq();
    del_tcb_rdy(pcurtcb);
    pcurtcb->prio = NewPrio;
    add_tcb_rdy(pcurtcb);
    mac_enable_irq();

#ifndef UNIT_DEBUG
    scheduler();
#endif
}



/**
 * Sleep - Make a task to sleep.
 * @return:
 *
 * @notes:
 */
void osSleep(void)
{
    list_t *plist;

    mac_disable_irq();
    pcurtcb->status = T_SLEEP;
    plist = del_tcb_rdy(pcurtcb);
    if(plist == NULL)
    {
        mac_enable_irq();
        return;
    }
    add_node_list_rear(&psleep, plist);
    mac_enable_irq();

#ifndef UNIT_DEBUG
    scheduler();
#endif
}



/**
 * osWakeUp - Wake up a sleep task.
 * @TaskID - The ID of sleep task.
 * @return:
 *
 * @notes:
 */
void osWakeUp(int32 TaskID)
{
    tcb_t *ptcb;
    list_t *plist = psleep.next;

    mac_disable_irq();
    do {
        ptcb = mac_find_entry(plist, tcb_t, link);
        if((ptcb->id == TaskID) && (ptcb->status == T_SLEEP)) {
            ptcb->status = T_READY;
            plist = del_node_list(&psleep, &ptcb->link);
            add_tcb_rdy(mac_find_entry(plist, tcb_t, link));
            mac_enable_irq();
#ifndef UNIT_DEBUG
            scheduler();
#endif
            break;
        }
        plist = plist->next;
    } while(plist != psleep.next);
}



/**
 * osWait - Make a task to been delayed for while.
 * @DelayTime - How long the task will be delayed.
 * @return:
 *
 * @notes:
 */
void osWait(uint32 DelayTime)
{
    list_t *plist = NULL;

    if(DelayTime > 0) {
        mac_disable_irq();
        pcurtcb->status = T_DELAY;
        pcurtcb->delay_time = DelayTime;

        plist = del_tcb_rdy(pcurtcb);
        if(plist != NULL) {
            add_node_list_rear(&pdelay, plist);
            mac_enable_irq();

#ifndef UNIT_DEBUG
            scheduler();
#endif
        }
    }
}



/**
 * osKill - Finish a life of a task.
 * @return:
 *
 * @notes:
 */
void osKill(void)
{
    mac_disable_irq();
    del_tcb_rdy(pcurtcb);
    del_node_list(&task, &pcurtcb->task_link);

    // Task was created by osInitTask()
    if((int32)pcurtcb->pstart >= get_start_address()) {
        reclaim(pcurtcb->pstart);
        reclaim(pcurtcb);
    }
    pcurtcb = NULL;
    mac_enable_irq();

#ifndef UNIT_DEBUG
    scheduler();
#endif
}



/**
 * osGetTaskHead - Get the head of task list.
 * @return: The head of task list.
 *
 * @notes:
 */
list_t *osGetTaskHead(void)
{
    return(&task);
}



/**
 * osVersion - Get the head sleep queue.
 * @return: The head sleep queue.
 *
 * @notes:
 */
list_t *osGetSleepHead(void)
{
    return(&psleep);
}



/**
 * osVersion - Get the head delay queue.
 * @return: The head delay queue.
 *
 * @notes:
 */
list_t *osGetDelayHead(void)
{
    return(&pdelay);
}



/**
 * osVersion - Return the current version of Bycore.
 * @return: The current version of Bycore.
 *
 * @notes:
 */
uint32 osVersion(void)
{
    return(VERSION);
}



/**
 * systick - The heart of Bycore, handle the system tick.
 * @return:
 *
 * @notes: It only refreshes deleyed time of all delayed tasks In this routines.
 */
void systick(void)
{
    tcb_t *ptcb;
    list_t *plist;
    list_t *plistnext;

    sysclock++;

    if((pcurtcb != NULL) && (pcurtcb->status == T_READY)) {
        pcurtcb->exe_time++;
    }
    plist = pdelay.next;
    if(plist !=  &pdelay) {
        do {
            ptcb = mac_find_entry(plist, tcb_t, link);
            if(--ptcb->delay_time <= 0) {
                ptcb->status = T_READY;
                plistnext = plist->next;
                plist = del_node_list(&pdelay, plist);
                add_tcb_rdy(ptcb);
                if(plistnext != plist) {
                    plist = plistnext;
                } else {
                    plist = &pdelay;
                }
            } else {
                plist = plist->next;
            }
        } while((plist != pdelay.next) && (plist != &pdelay));
    }
}



/**
 * lock_schedule -Locked schedule.
 * @return:
 *
 * @notes:
 */
void lock_schedule(void)
{
    mac_disable_irq();
    is_lock_schedule = TRUE;
    mac_enable_irq();
}



/**
 * unlock_schedule - Unlocked schedule.
 * @return:
 *
 * @notes:
 */
void unlock_schedule(void)
{
    mac_disable_irq();
    is_lock_schedule = FALSE;
    mac_enable_irq();

}


/**
 * get_clock - Get the number of clock count.
 * @return: The number of clock count.
 *
 * @notes:
 */
uint32 get_clock(void)
{
    return(sysclock);
}



/**
 * current - Get the TCB of current task.
 * @return: The TCB of current task.
 *
 * @notes:
 */
tcb_t *current(void)
{
    return(pcurtcb);
}

